#include "sseFunc.h"
#define IDEAL_ALIGNMENT 16
#include "define.h"

using namespace std;

static int SIMD_LEN;

template <class T>
void init_SSE(){
	SIMD_LEN = (IDEAL_ALIGNMENT / sizeof(double));
}

template <>
void init_SSE<float> (){
	SIMD_LEN = (IDEAL_ALIGNMENT / sizeof(float));
}

template <class T, class U, class V>
void compexp_SSE(const int ns, const int nt, T const xs[], T const ys[], T const zs[],
									 T const xt[], T const yt[], T const zt[], complex<T> const den[], complex<T> pot[],  V kernel_coef) {
#ifdef USE_COMP_COEF
	U coeff = _sse_set1<T,U> (-1 * real(kernel_coef));
	U coeff_imag = _sse_set1<T,U> (imag(kernel_coef));
#else
	U coeff = _sse_set1<T,U> (-1 * kernel_coef);
#endif

	complex<T> i_imag = complex<T> (0.0, -1.0);
	complex<T> sse_mult_coeff = complex<T>(i_imag * kernel_coef);
	register complex<T> *z = new complex<T>[ns];
#ifdef USE_COMP_COEF
	register T *a = new T[ns];
	register T *b = new T[ns];
#else
	register T *param = new T[ns];
#endif
	register T *rhos = new T[ns];
	register T *p1 = new T[SIMD_LEN];
	U p_temp;
	complex<T> p;

	for(int i = 0; i < nt; i++) {
		U tx = _sse_load1<T, U> (&xt[i]);
		U ty = _sse_load1<T, U> (&yt[i]);
		U tz = _sse_load1<T, U> (&zt[i]);

		p_temp = _sse_setzero<U>();
		p = complex<T>(0,0);

		int j = 0;
		unsigned long t = (unsigned long) xs;
		 if(t % 16 != 0 ) {
				T rx = xs[j] - xt[i];
				T ry = ys[j] - yt[i];
				T rz = zs[j] - zt[i];
				T prho = rx*rx + ry*ry + rz*rz;
				prho = sqrt(prho);
				complex<T> Kernel = exp(sse_mult_coeff * prho);
				INVSQRT(prho);
				p += (Kernel * den[j]) * prho;
				j++;
		 }

		int align = j;
		int length = ((ns - j) / SIMD_LEN);
		length = (length *  SIMD_LEN);

		for(; j + SIMD_LEN < ns  ; j += SIMD_LEN) {
			U sx,sy,sz;
			sx = _sse_load<T,U> (&xs[j]);
			sy = _sse_load<T,U> (&ys[j]);
			sz = _sse_load<T,U> (&zs[j]);
			
			U dX = _sse_sub<U> (tx , sx);
			U dY = _sse_sub<U> (ty , sy);
			U dZ = _sse_sub<U> (tz , sz);
			
			sx = _sse_mul<U> (dX, dX);
			sy = _sse_mul<U> (dY, dY);
			sz = _sse_mul<U> (dZ, dZ);
			
			U rho = _sse_add<U> (sx, sy);
			rho = _sse_add<U> (rho, sz);
			
			rho = _sse_sqrt<U>(rho);
			
#ifdef USE_COMP_COEF
			U kernel_param = _sse_mul<U>(rho, coeff);
			U kernel_param_imag = _sse_mul<U>(rho, coeff_imag);
#else
			U kernel_param = _sse_mul<U>(rho, coeff);
#endif
			
			
#ifdef USE_COMP_COEF
			_sse_store<T,U> (&a[j - align], kernel_param);
			_sse_store<T,U> (&b[j - align], kernel_param_imag);
#else
			_sse_store<T,U> (&(param[j - align]), kernel_param);
#endif
			
			_sse_store<T,U> (&(rhos[j - align]), rho);
		}
		

#ifdef USE_COMP_COEF
		_sse_CIS<T>(length, a, z);
		_sse_EXP<T>(length, b, a);
#else
		_sse_CIS<T>(length, param, z);
#endif

		for(j = align; (j + SIMD_LEN) < ns  ; j += SIMD_LEN) {
			U kernel_real, kernel_imag, den_real, den_imag, kernel_const;
			
#ifdef USE_PD
	#ifdef USE_COMP_COEF
				kernel_const = _sse_set<T,U> (a[j+1 - align], a[j - align]);
	#endif
				kernel_real = _sse_set<T,U> (real(z[j+1 - align]),real(z[j - align]));
				kernel_imag = _sse_set<T,U> (imag(z[j+1-align]),imag(z[j-align]));
				den_real = _sse_set<T,U> (real(den[j+1]),real(den[j]));
				den_imag = _sse_set<T,U> (imag(den[j+1]),imag(den[j]));
#else
	#ifdef USE_COMP_COEF
				kernel_const = _sse_set<T,U> (a[j+3 - align], a[j+2 - align], a[j+1 - align], a[j - align]);
	#endif
				kernel_real = _sse_set<T,U> (real(z[j+3 - align]), real(z[j+2 - align]), real(z[j+1 - align]),real(z[j - align]));
				kernel_imag = _sse_set<T,U> (imag(z[j+3 - align]), imag(z[j+2 - align]), imag(z[j+1 - align]),imag(z[j - align]));
				den_real = _sse_set<T,U> (real(den[j+3]), real(den[j+2]),real(den[j+1]),real(den[j]));
				den_imag = _sse_set<T,U> (imag(den[j+3]), imag(den[j+2]),imag(den[j+1]),imag(den[j]));
#endif

			U temp, p_real, p_imag;
			p_real = _sse_mul<U> (kernel_real,den_real);
			temp = _sse_mul<U> (kernel_imag,den_imag);
			p_real = _sse_sub<U> (p_real, temp);
			p_imag = _sse_mul<U> (kernel_real,den_imag);
			temp = _sse_mul<U> (kernel_imag,den_real);
			p_imag = _sse_add<U> (p_imag, temp);
	
			U rho_temp;

			rho_temp = _sse_load<T,U> (&rhos[j- align]);
			p_real = _sse_div<U> (p_real, rho_temp);
			p_imag = _sse_div<U> (p_imag, rho_temp);
#ifdef USE_COMP_COEF
			p_real = _sse_mul<U> (p_real, kernel_const);
			p_imag = _sse_mul<U> (p_imag, kernel_const);
#endif
			p_real = _sse_hadd<U> (p_real, p_imag);
			p_temp = _sse_add<U> (p_real, p_temp);
		}

		_sse_store<T,U> (p1, p_temp);

#ifdef USE_PD
			p += complex<T>(p1[0],p1[1]);
#else
			p += complex<T>(p1[0]+p1[1], p1[2]+p1[3]);
#endif

		while( j < ns) {
				T rx= xs[j] - xt[i];
				T ry=ys[j] - yt[i];
				T rz=zs[j] - zt[i];
				T prho = rx*rx + ry*ry + rz*rz;
				prho = sqrt(prho);
				complex<T> Kernel = exp(sse_mult_coeff * prho);
				INVSQRT(prho);
				p += (Kernel * den[j]) * prho;
				j++;
		}
		pot[i] = p;
		
	}
  return;
}
